#include "reshoughtools.h"

float distanceEuclidian ( CircleTree& c1, CircleTree& c2 )
{
    vcg::Point3f c1c2 = c2.getCenter() - c1.getCenter();

    return ( c1c2.Norm() );
}

float distancePfeifer ( CircleTree& c1, CircleTree& c2)
{
    vcg::Point3f s1 = c1.getCenter ();
    vcg::Point3f e1 = s1 + c1.getNormal ();
    vcg::Point3f s2 = c2.getCenter ();
    vcg::Point3f e2 = s2 + c2.getNormal ();

    float distS1C2 = distancePointDroite ( s1, s2, c2.getNormal () );
    float distE1C2 = distancePointDroite ( e1, s2, c2.getNormal () );
    float distS2C1 = distancePointDroite ( s2, s1, c1.getNormal () );
    float distE2C1 = distancePointDroite ( e2, s1, c1.getNormal () );

    return ( 0.25 * ( distE1C2 + distE2C1 + distS1C2 + distS2C1 ) );
}

float distanceSumOfAngles(CircleTree &c1, CircleTree &c2)
{
    vcg::Point3f normal1 = c1.getNormal();
    vcg::Point3f normal2 = c2.getNormal();
    vcg::Point3f c1c2 = c2.getCenter() - c1.getCenter();

    float rslt = angle( normal1, normal2 );
    rslt += angle( normal1, c1c2 );
    rslt += angle( normal2, c1c2 );

    return rslt;
}

float distanceMinAngle( CircleTree& c1, CircleTree& c2 )
{
    vcg::Point3f normal1 = c1.getNormal();
    vcg::Point3f normal2 = c2.getNormal();
    vcg::Point3f c1c2 = c2.getCenter() - c1.getCenter();

    float angle1 = angle( normal1, c1c2 );
    float angle2 = angle( normal2, c1c2 );

    if ( angle1 < angle2 )
    {
        return angle1;
    }

    else
    {
        return angle2;
    }

    return 0; // Pour eviter un warning a la compilation, on ne passe jamais par cette instruction
}

float distanceParallelogramIntersectionArea ( CircleTree& c1, CircleTree& c2 )
{
    vcg::Point3f normal1 = c1.getNormal();
    vcg::Point3f normal2 = c2.getNormal();
    vcg::Point3f c1c2 = c2.getCenter() - c1.getCenter();
    float d = c1c2.Norm();

    // TEMPORAIRE on diminue les normales pour ne pas depasser le cas limite
    normal1 /= 1000;
    normal2 /= 1000;
    float norm = normal1.Norm();

    if ( d < 2 * norm )
    {
        qDebug() << "Cas limite depasse";
        assert(0);
        return 1000;
    }


    // on oriente les normales comme il faut
    if( scalarProduct( c1c2, normal1 ) > 0 )
    {
        normal1 *= -1;
    }

    if( scalarProduct( c1c2, normal2 ) < 0 )
    {
        normal2 *= -1;
    }

    // On calcule les angles alpha et beta
    float alpha = angle( normal1, c1c2 );
    float beta = angle( normal2, c1c2 );

    // On les ordonne (alpha devant etre le plus petit)
    if ( alpha > beta )
    {
        float tmp = beta;
        beta = alpha;
        alpha = tmp;
    }

    // Calcul de l'aire
    return ( d + ( ( 0.5 * norm * sin( alpha) ) * ( 2*d - norm * cos(alpha) - ( norm * sin(alpha) / tan(beta) ) ) ) );
}

float distancePointDroite (vcg::Point3f a, vcg::Point3f b, vcg::Point3f u )
{
    // Calcule la distance entre le point a et la droite portee par b de vecteur directeur u
    vcg::Point3f vectorBA = b - a;
    vcg::Point3f vectorCrossProduct = crossProduct ( vectorBA, u );
    float normCrossProduct = vectorCrossProduct.Norm();
    float normU = u.Norm();

    return ( normCrossProduct / normU );
}

vcg::Point3f crossProduct (  vcg::Point3f p1, vcg::Point3f p2 )
{
    // Calcule le produit vectoriel de deux vecteurs
    vcg::Point3f rslt;

    rslt[0] = p1[1]*p2[2] - p1[2]*p2[1];
    rslt[1] = p1[2]*p2[0] - p1[0]*p2[2];
    rslt[2] = p1[0]*p2[1] - p1[1]*p2[0];

    return rslt;
}
